/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "PBF刚体解算.h"

#include "core/CUDA_utils.cuh"

#include <线性代数/矢量计算.h>
#include <线性代数/矩阵计算.h>
#include <matXX.h>
#include <四元数.h>





__global__ void f_rigidbodyParticleFill(
	uint32* c每个刚体粒子起始,
	uint16* c每个刚体粒子数
) {
	uint64 当前ID = blockIdx.x * blockDim.x + threadIdx.x;
	

}


__global__ void f_rigidbodyParticleWidgets(
	vec3* c位置old,
	vec3* c刚体粒子,
	float32* c质量,
	uint32* c每个刚体粒子起始,
	uint16* c每个刚体粒子数
) {
	//__shared__  float32 a[512];
	//uint64 当前ID = blockIdx.x * blockDim.x + threadIdx.x;
	uint64 当前ID = blockIdx.x;
	//printf(" 权重和 = %i \n", 当前ID);
	
//#pragma unroll 3
	uint32 起始索引 = c每个刚体粒子起始[当前ID];
	//uint32 num = 起始索引 + c每个刚体粒子数[当前ID];
	uint32 num = c每个刚体粒子数[当前ID];

	vec3 中心坐标0 = {};
	vec3 中心坐标 = {};
	

	float32 权重和 = 0;
	for (uint32 i = 起始索引 ; i < num; ++i) {
		float32 权重 = 1.0f / (c质量[i] + DEF_防止除0补偿值);
		vec_add(&中心坐标0, vec_multi(c位置old[i], 权重));
		vec_add(&中心坐标, vec_multi(c刚体粒子[i], 权重));
		权重和 += 权重;
	}

	//printf(" 权重和 =  %f %i \n", 权重和, 当前ID);
	//return;
	if (权重和 != 0) {
		//__shared__  vec3 q[512];
		vec_div(&中心坐标0, 权重和);
		vec_div(&中心坐标, 权重和);

		vec4 quat = { 0,0,0,1 };



		float32 角度 = 0;
		//float32 最小 = 0;
		//float32 最大 = 0;
		float32 强度 = 0;
		vec3 撞击方向 = {};
		//printf(" num =  %i %i \n", 起始索引, num);
		for (uint32 i = 起始索引; i < num; ++i) {
			float32 权重 = 1.0f / (c质量[i] + DEF_防止除0补偿值);

			vec3 q0 = vec_sub(c刚体粒子[i], c位置old[i]);
			vec_multi(&q0, 权重);
			强度 += vec_normalize(&q0);

			vec3 q = vec_sub(c刚体粒子[i], 中心坐标);
			vec_normalize(&q);
			//vec_multi(&q, 权重);

			vec_add(&撞击方向, vec_cross(q0, q));
			//vec_add(&撞击方向, vec_multi(vec_cross(q0, q), l));

			权重 = vec_dot(q0, q);
			if (权重 >= 0) 角度 += 1.0 - 权重;
			else 角度 += -(1.0 + 权重);
			//角度 += vec_dot(q0, q);
			//最小 = fminf(最小, 权重);
			//最大 = fmaxf(最大, 权重);
		}

		float32 l = vec_normalize(&撞击方向);
		//vec_multi(&撞击方向, 角度); 
		//float32 l = vec_normalize(&撞击方向);
		//l = (1.0 - 最大) + (1.0 + 最小);
		if (l > DEF_防止除0补偿值) {
			quat.x = 撞击方向.x;
			quat.y = 撞击方向.y;
			quat.z = 撞击方向.z;
			quat.w = 角度 * (1.0 / (强度 + DEF_防止除0补偿值)) * 0.001;

			for (uint32 i = 起始索引; i < num; ++i) {
				c刚体粒子[i] = vec_add(中心坐标, vec_绕轴旋转(quat, vec_sub(c位置old[i], 中心坐标0)));
			}

		}
		

		return;



		mat3X3 mat0; f_mat_zero(mat0);
		mat3X3 mat; f_mat_zero(mat0);
		for (uint32 i = 起始索引; i < num; ++i) {
			vec3 q = vec_sub(c位置old[i], 中心坐标0);
			vec3 p = vec_sub(c刚体粒子[i], 中心坐标);

			float32 权重 = 1.0f / (c质量[i] + DEF_防止除0补偿值);
			vec_multi(&q, 权重);
			vec_multi(&p, 权重);


			mat0.row0.x += q.x * q.x;
			mat0.row0.y += q.x * q.y;
			mat0.row0.z += q.x * q.z;

			mat0.row1.x += q.y * q.x;
			mat0.row1.y += q.y * q.y;
			mat0.row1.z += q.y * q.z;

			mat0.row2.x += q.y * q.x;
			mat0.row2.y += q.y * q.y;
			mat0.row2.z += q.y * q.z;



			mat.row0.x += p.x * q.x;
			mat.row0.y += p.x * q.y;
			mat.row0.z += p.x * q.z;

			mat.row1.x += p.y * q.x;
			mat.row1.y += p.y * q.y;
			mat.row1.z += p.y * q.z;

			mat.row2.x += p.y * q.x;
			mat.row2.y += p.y * q.y;
			mat.row2.z += p.y * q.z;

		}
		//mat0 = f_mat3x3_T(mat0);
		mat = f_mat_multi(mat0, mat);

		
		mat3X3 R;
		for (uint8 iter = 0; iter < 3; ++iter) {
			R = f_mat3x3_quat(quat);
			printf(" w =  %f  %f %f %i \n", quat.x, quat.y, quat.z, iter);
			vec3 旋转轴向 = vec_cross(R.row0, mat.row0);
			vec_add(&旋转轴向, vec_cross(R.row1, mat.row1));
			vec_add(&旋转轴向, vec_cross(R.row2, mat.row2));
			printf(" w =  %f  %f %f %i \n", 旋转轴向.x, 旋转轴向.y, 旋转轴向.z, iter);
			float32 w = vec_dot(R.row0, mat.row0) + DEF_防止除0补偿值;
			w *= vec_dot(R.row1, mat.row1);
			w *= vec_dot(R.row2, mat.row2);
			w = 1.0f / fabs(w);
			
			vec_multi(&旋转轴向, w);

			w = vec_normalize(&旋转轴向);
			if (w < DEF_防止除0补偿值) {
				break;
			}
			quat = f_quat_轴旋转(旋转轴向, w);
			vec_normalize(&quat);
		}
		//return;
		
		for (uint32 i = 起始索引; i < num; ++i) {
			c刚体粒子[i] = vec_add(中心坐标, f_quat_Rotate(quat, vec_sub(c位置old[i], 中心坐标0)));
			//printf("c刚体粒子 x =  %f %f %f \n", c刚体粒子[i].x, c刚体粒子[i].y, c刚体粒子[i].z);
		}

		return;



		//--------------------------------- clear code -------------------------------------
		
	}
}



__global__ void f_rigidbodyParticleSim(
	vec3* c刚体粒子,
	uint32* c每个刚体粒子起始,
	uint32* c每个刚体粒子数,
	vec4* cQuat,
	float32* c权重和,
	float32* c质量

) {
	uint64 当前ID = blockIdx.x * blockDim.x + threadIdx.x;

	__shared__  vec3 q[1024];


}







void f_CUDA_update刚体区间(S_PBF_Gpu& pbf, uint64 粒子数, uint32* 每个刚体粒子起始, uint16* 每个刚体粒子数, uint16 批次) {
	if (pbf.m_cu刚体起始索引) cudaFree(pbf.m_cu刚体起始索引);
	if (pbf.m_cu刚体粒子数量) cudaFree(pbf.m_cu刚体粒子数量);


	COUDA_CALL(cudaMalloc((void**)&(pbf.m_cu刚体起始索引), sizeof(uint32) * 粒子数));
	COUDA_CALL(cudaMalloc((void**)&(pbf.m_cu刚体粒子数量), sizeof(uint16) * 粒子数));

	cudaMemcpy(pbf.m_cu刚体起始索引, 每个刚体粒子起始, 粒子数 * sizeof(uint32), cudaMemcpyKind::cudaMemcpyHostToDevice);
	cudaMemcpy(pbf.m_cu刚体粒子数量, 每个刚体粒子数, 粒子数 * sizeof(uint16), cudaMemcpyKind::cudaMemcpyHostToDevice);


}

void f_CUDA_拷贝刚体粒子初始坐标(S_PBF_Gpu& pbf, uint64 粒子数, vec3* pos)
{
	if (pbf.m_cu刚体初始位置) cudaFree(pbf.m_cu刚体初始位置);

	COUDA_CALL(cudaMalloc((void**)&(pbf.m_cu刚体初始位置), sizeof(vec3) * 粒子数));
	COUDA_CALL(cudaMemcpy(pbf.m_cu刚体初始位置, pos, 粒子数 * sizeof(vec3), cudaMemcpyKind::cudaMemcpyHostToDevice));
}



void f_CUDA_update刚体粒子(S_PBF_Gpu& pbf, uint32 刚体数量, uint16 批次) {
	//auto& points0 = *DEF_Vec3_1D插座数据(f_get输入插座(1));
	//auto& points = *DEF_Vec3_1D插座数据(f_get输入插座(2));
	//auto& 质量 = *DEF_F32_1D插座数据(f_get输入插座(3));
	if (!刚体数量) return;

	//数据拷贝到GPU
	f_rigidbodyParticleWidgets <<< {刚体数量, 1, 1}, { 批次, 1, 1} >>> (
		pbf.m_cu前一帧位置,
		//pbf.m_cu刚体初始位置,
		pbf.m_cu位置,
		pbf.m_cu质量, 
		pbf.m_cu刚体起始索引,
		pbf.m_cu刚体粒子数量
	);

	cudaDeviceSynchronize();

}






